#include "camodocal/gpl/EigenQuaternionParameterization.h"

#include <cmath>

namespace camodocal
{

    bool
    EigenQuaternionParameterization::Plus(const double *x,
                                          const double *delta,
                                          double *x_plus_delta) const
    {
        const double norm_delta =
            sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
        if (norm_delta > 0.0)
        {
            const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
            double q_delta[4];
            q_delta[0] = sin_delta_by_delta * delta[0];
            q_delta[1] = sin_delta_by_delta * delta[1];
            q_delta[2] = sin_delta_by_delta * delta[2];
            q_delta[3] = cos(norm_delta);
            EigenQuaternionProduct(q_delta, x, x_plus_delta);
        }
        else
        {
            for (int i = 0; i < 4; ++i)
            {
                x_plus_delta[i] = x[i];
            }
        }
        return true;
    }

    bool
    EigenQuaternionParameterization::ComputeJacobian(const double *x,
                                                     double *jacobian) const
    {
        jacobian[0] = x[3];
        jacobian[1] = x[2];
        jacobian[2] = -x[1]; // NOLINT
        jacobian[3] = -x[2];
        jacobian[4] = x[3];
        jacobian[5] = x[0]; // NOLINT
        jacobian[6] = x[1];
        jacobian[7] = -x[0];
        jacobian[8] = x[3]; // NOLINT
        jacobian[9] = -x[0];
        jacobian[10] = -x[1];
        jacobian[11] = -x[2]; // NOLINT
        return true;
    }

}
